import serial
import time
import json
from .base_vehicle_controller import BaseVehicleController
from .controller_registry import ControllerRegistry
from .config import CONTROLLER_CONFIG

@ControllerRegistry.register("SteeringDrive")
class SteeringDriveController(BaseVehicleController):
    def __init__(self):
        super().__init__()
        self.last_cmd_time = 0
        self.max_speed = CONTROLLER_CONFIG['max_speed']
        self.current_angle = 90  # 初始居中角度
        self.controller_type = "SteeringDrive"
        
    def send_command(self, command):
        try:
            current_time = time.time()

            # 处理速度命令
            if 'speed' in command:
                speed = int(command['speed'])
                speed = max(-self.max_speed, min(self.max_speed, speed))
                self.arduino.write(f"SET_SPEED {speed}\n".encode())

            # 处理转向命令
            if 'angle' in command:
                angle = int(command['angle'])
                angle = max(0, min(180, angle))
                self.current_angle = angle
                self.arduino.write(f"SET_ANGLE {angle}\n".encode())

            self.last_cmd_time = current_time

        except Exception as e:
            print(f"An error occurred while sending command: {e}")

    def emergency_stop(self):
        try:
            self.arduino.write("STOP\n".encode())
        except Exception as e:
            print(f"An error occurred during emergency stop: {e}")

    def get_current_status(self):
        return {
            'controller_type': self.controller_type,
            'last_cmd_time': self.last_cmd_time,
            'current_angle': self.current_angle,
            'max_speed': self.max_speed
        }

    def get_controller_type(self):
        return self.controller_type
